#include <unistd.h>//Linux系统下网络通讯的头文件集合
#include <sys/types.h>
#include <sys/socket.h>
#include <netdb.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>
#include <errno.h>
#include <malloc.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <sys/ioctl.h>
#include <stdarg.h>
#include <fcntl.h>
#include <fcntl.h>
#include <iostream>
#include "../include/i2c_driver.h"
#include "../include/ICM20948_driver.h"
#include "../include/car_IMU_upload.h"
#include <math.h>

using namespace std;

enum
{
	PORT = 0X4321
};

struct IMU
{
    float accelX;
    float accelY;
    float accelZ;
    float gyroX;
    float gyroY;
    float gyroZ;
};

void IMU_upload()
{
	int m_sockClient;
	IMU imu;
	memset((void*)&imu, 0, sizeof(imu));
	if ((m_sockClient = socket(AF_INET, SOCK_DGRAM, 0)) < 0)    //创建socket句柄，采用UDP协议
	{
		printf("create socket error: %s(errno: %d)\n", strerror(errno), errno);
		return ;
	}
	sockaddr_in m_servaddr;
	memset(&m_servaddr, 0, sizeof(m_servaddr));  //初始化结构体
	m_servaddr.sin_family = AF_INET;           //设置通信方式
	m_servaddr.sin_port = htons(PORT);         //设置端口号

	bind(m_sockClient, (sockaddr*)&m_servaddr, sizeof(m_servaddr));//绑定端口号

        int i2cfd  = open("/dev/i2c-2", O_RDWR);
        if (i2cfd < 0)
            exit(EXIT_FAILURE);

        ioctl(i2cfd, I2C_SLAVE, SLAVE_ADDR);

        ICM20948_PowerOn(i2cfd, UB0_PWR_MGMNT_1_DEV_RESET);

        ICM20948_PowerOn(i2cfd, UB0_PWR_MGMT_1_AUTO_CLOCK);

        ICM20948_GyroConfigInit(i2cfd, UB2_GYRO_CONFIG_1_FS_SEL_1000DPS, 
            UB2_GYRO_CONFIG_1_DLPFCFG_12HZ);
        
        ICM20948_AccelConfigInit(i2cfd, UB2_ACCEL_CONFIG_FS_SEL_8G,
            UB2_ACCEL_CONFIG_DLPF_BANDWIDTH_111HZ);

        ICM20948_EnableI2cMaster(i2cfd);

        ICM20948_AccelGyroOnOrOff(i2cfd, UB0_PWR_MGMT2_ACCEL_GYRO_ON);

        ICM20948_InitMag(i2cfd);

	while (true)
	{
               
	    ICM20948_GetAccel(i2cfd, &imu.accelX, &imu.accelY, &imu.accelZ);
	    ICM20948_GetGyro(i2cfd, &imu.gyroX, &imu.gyroY, &imu.gyroZ);
	    printf("acc: %f %f %f \n", imu.accelX, imu.accelY, imu.accelZ);
	    printf("gyr: %f %f %f \n", imu.gyroX, imu.gyroY, imu.gyroZ);
		
		m_servaddr.sin_addr.s_addr = inet_addr("192.168.2.1");
		m_servaddr.sin_port = htons(0x1234);//设置需要发送的IP和端口号
		//cout << "1" << endl;
		//sendto(m_sockClient, encodeImg, nSize, 0, (const sockaddr*)& m_servaddr, sizeof(m_servaddr));
		int send = sendto(m_sockClient, (void*)&imu, sizeof(imu), 0, (const sockaddr*)& m_servaddr, sizeof(m_servaddr));
		cout << "IMU send: "<<send << endl;
		//memset(&encodeImg, 0, sizeof(encodeImg));  //初始化结构体
	}
	close(i2cfd);
}

